
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_motors.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       电机模块初始化
  * @param[in]   pMotors  
  * @param[in]   loop_rate  
  * @param[in]   speed_hz  
  * @param[out]  
  * @retval      
  * @note        
  */
void Motors(Motors_HandleTypeDef *pMotors, uint16_t loop_rate, uint16_t speed_hz)
{
    pMotors->_loop_rate = loop_rate;
    pMotors->_speed_hz  = speed_hz;
    pMotors->_spool_desired  = MOTOR_DESIRED_SHUT_DOWN;
    pMotors->_spool_state    = MOTOR_SHUT_DOWN;
    pMotors->_air_density_ratio    = 1.0f;

    lpf_set_cutoff1(&pMotors->_throttle_filter, 0.0f);
    lpf_reset(&pMotors->_throttle_filter, 0.0f);

    // init limit flags
    pMotors->limit.roll = true;
    pMotors->limit.pitch = true;
    pMotors->limit.yaw = true;
    pMotors->limit.throttle_lower = true;
    pMotors->limit.throttle_upper = true;
    pMotors->_thrust_boost = false;
    pMotors->_thrust_balanced = true;
}

void Motors_set_desired_spool_state(Motors_HandleTypeDef *pMotors, DesiredSpoolState spool)
{
    if (pMotors->_armed || (spool == MOTOR_DESIRED_SHUT_DOWN)) {
        pMotors->_spool_desired = spool;
    }
}

DesiredSpoolState Motors_get_desired_spool_state(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_spool_desired;
}

SpoolState Motors_get_spool_state(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_spool_state;
}

// set_density_ratio - sets air density as a proportion of sea level density
void Motors_set_air_density_ratio(Motors_HandleTypeDef *pMotors, float ratio)
{
    pMotors->_air_density_ratio = ratio;
}

// set limit flag for pitch, roll and yaw
void Motors_set_limit_flag_pitch_roll_yaw(Motors_HandleTypeDef *pMotors, bool flag)
{
    pMotors->limit.roll  = flag;
    pMotors->limit.pitch = flag;
    pMotors->limit.yaw   = flag;
}

// set update rate to motors - a value in hertz
void Motors_set_update_rate(Motors_HandleTypeDef *pMotors, uint16_t speed_hz )
{
    pMotors->_speed_hz = speed_hz;
}

// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
void Motors_set_radio_passthrough(Motors_HandleTypeDef *pMotors, float roll_input, float pitch_input, float throttle_input, float yaw_input)
{
    pMotors->_roll_radio_passthrough = roll_input;
    pMotors->_pitch_radio_passthrough = pitch_input;
    pMotors->_throttle_radio_passthrough = throttle_input;
    pMotors->_yaw_radio_passthrough = yaw_input;
}

// set loop rate. Used to support loop rate as a parameter
void Motors_set_loop_rate(Motors_HandleTypeDef *pMotors, uint16_t loop_rate)
{
    pMotors->_loop_rate = loop_rate;
}

// return the roll factor of any motor, this is used for tilt rotors and tail sitters
// using copter motors for forward flight
float Motors_get_roll_factor(Motors_HandleTypeDef *pMotors, uint8_t i)
{
    return 0.0f;
}

// This function required for tradheli. Tradheli initializes targets when going from unarmed to armed state.
// This function is overriden in motors_heli class.   Always true for multicopters.
bool Motors_init_targets_on_arming(Motors_HandleTypeDef *pMotors)
{
    return true;
}

motors_pwm_type Motors_get_pwm_type(Motors_HandleTypeDef *pMotors)
{
    return (motors_pwm_type)pMotors->_pwm_type; 
}

void Motors_rc_write(Motors_HandleTypeDef *pMotors, uint8_t chan, uint16_t pwm)
{
    if (chan < GP_MOTORS_MAX_NUM_MOTORS) {
        pMotors->_motor_output_pwm[chan] = pwm;
    }
}

// add a motor to the motor map
void Motors_add_motor_num(Motors_HandleTypeDef *pMotors, int8_t motor_num)
{
    uint8_t motor_quantity = motor_num + 1;

    if (motor_quantity > pMotors->_motor_quantity) {
        pMotors->_motor_quantity = motor_quantity;
    }
}

// check initialisation succeeded
bool Motors_initialised_ok(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_initialised_ok;
}

void Motors_set_initialised_ok(Motors_HandleTypeDef *pMotors, bool val)
{
    pMotors->_initialised_ok = val;
}

// arm, disarm or check status status of motors
bool Motors_get_armed(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_armed;
}

void Motors_set_armed(Motors_HandleTypeDef *pMotors, bool arm)
{
    if (pMotors->_armed != arm) {
        pMotors->_armed = arm;

        if (!arm) {
            //MotorsMC_save_params_on_disarm(((MotorsMC_HandleTypeDef *))pMotors);
        }
    }
};

// set motor interlock status
void Motors_set_interlock(Motors_HandleTypeDef *pMotors, bool set)
{
    pMotors->_interlock = set;
}

// get motor interlock status.  true means motors run, false motors don't run
bool Motors_get_interlock(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_interlock;
}

// set_roll, set_pitch, set_yaw, set_throttle
void Motors_set_roll(Motors_HandleTypeDef *pMotors, float roll_in)
{
    pMotors->_roll_in = roll_in;
};        // range -1 ~ +1

void Motors_set_roll_ff(Motors_HandleTypeDef *pMotors, float roll_in)
{
    pMotors->_roll_in_ff = roll_in;
};    // range -1 ~ +1

void Motors_set_pitch(Motors_HandleTypeDef *pMotors, float pitch_in)
{
    pMotors->_pitch_in = pitch_in;
};    // range -1 ~ +1

void Motors_set_pitch_ff(Motors_HandleTypeDef *pMotors, float pitch_in)
{
    pMotors->_pitch_in_ff = pitch_in;
};  // range -1 ~ +1

void Motors_set_yaw(Motors_HandleTypeDef *pMotors, float yaw_in)
{
    pMotors->_yaw_in = yaw_in;
};            // range -1 ~ +1

void Motors_set_yaw_ff(Motors_HandleTypeDef *pMotors, float yaw_in)
{
    pMotors->_yaw_in_ff = yaw_in;
};      // range -1 ~ +1

void Motors_set_throttle(Motors_HandleTypeDef *pMotors, float throttle_in)
{
    pMotors->_throttle_in = throttle_in;
};   // range 0 ~ 1

void Motors_set_throttle_avg_max(Motors_HandleTypeDef *pMotors, float throttle_avg_max)
{
    pMotors->_throttle_avg_max = math_constrain_float(throttle_avg_max, 0.0f, 1.0f);
};   // range 0 ~ 1

void Motors_set_throttle_filter_cutoff(Motors_HandleTypeDef *pMotors, float filt_hz)
{
    lpf_set_cutoff1(&pMotors->_throttle_filter,filt_hz);
}

void Motors_set_forward(Motors_HandleTypeDef *pMotors, float forward_in)
{
    pMotors->_forward_in = forward_in;
}; // range -1 ~ +1

void Motors_set_lateral(Motors_HandleTypeDef *pMotors, float lateral_in)
{
    pMotors->_lateral_in = lateral_in;
};     // range -1 ~ +1

// accessors for roll, pitch, yaw and throttle inputs to motors
float Motors_get_roll(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_roll_in;
}

float Motors_get_pitch(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_pitch_in;
}

float Motors_get_yaw(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_yaw_in;
}

float Motors_get_throttle_out(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_throttle_out;
}

float Motors_get_throttle(Motors_HandleTypeDef *pMotors)
{
    return math_constrain_float(lpf_get_output(&pMotors->_throttle_filter), 0.0f, 1.0f);
}

float Motors_get_throttle_bidirectional(Motors_HandleTypeDef *pMotors)
{
    return math_constrain_float(2 * (lpf_get_output(&pMotors->_throttle_filter) - 0.5f), -1.0f, 1.0f);
}

float Motors_get_forward(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_forward_in;
}

float Motors_get_lateral(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_lateral_in;
}

//float         Motors_get_throttle_hover(Motors_HandleTypeDef *pMotors);  //其他文件实现

// motor failure handling
void Motors_set_thrust_boost(Motors_HandleTypeDef *pMotors, bool enable)
{
    pMotors->_thrust_boost = enable;
}

bool Motors_get_thrust_boost(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_thrust_boost;
}

/*------------------------------------test------------------------------------*/


